#include "buffer.h"
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <time.h>
#include <iostream>
#include <cstdlib>
#include <strings.h>
#include <string.h>
#include <ctype.h>

using std::cout;
using std::endl;


Buffer::Buffer()
{
    this->initialize('x');
}

void Buffer::initialize(char c)
{
    this->carac = c;
    this->algoritmos.initialize();
    for(int i=0; i<1000000;i++)
    {
        rx_buffer[i] = 0;
    }

    for(int i=0; i < 200000; i++)
    {
        values_arrayX[i] = 0;
        values_arrayY[i] = 0;
        values_arrayZ[i] = 0;
    }

    values_write_indexX = 0;
    values_read_indexX = -1;
    values_write_indexY = 0;
    values_read_indexY = -1;
    values_write_indexZ = 0;
    values_read_indexZ = -1;
    read_index=0;
    write_index=0;
    continuar = true;
    calibrar = true;
}

void Buffer::stopThread()
{
    continuar = false;
}

int Buffer::validarToken(char* ptrVar, char option)
{
    if( (isxdigit(ptrVar[0]) &&
        isxdigit(ptrVar[1]) &&
        isxdigit(ptrVar[2]) &&
        isxdigit(ptrVar[3])) == 1)
    {
        int num = this->hexToInt(ptrVar);
        if(!calibrar)
        {
            algoritmos.integrar(num);
        }
        switch(option)
        {
            case 'x':
                values_arrayX[values_write_indexX] = num;
                values_write_indexX++;
                if(values_write_indexX == 64)
                {
                    algoritmos.calibrar(values_arrayX);
                    calibrar = false;
                }
                break;
            case 'y':
                values_arrayY[values_write_indexY] = num;
                values_write_indexY++;
                if(values_write_indexY == 64)
                {
                    algoritmos.calibrar(values_arrayY);
                    calibrar = false;
                }
                break;
            case 'z':
                values_arrayZ[values_write_indexZ] = num;
                values_write_indexZ++;
                if(values_write_indexZ == 64)
                {
                    algoritmos.calibrar(values_arrayZ);
                    calibrar = false;
                }
                break;
        }

        return 1;
    }
    else
    {
        //printf("\nnumeros mal\n");
        return 0;
    }
}

void Buffer::procesar_trama(){
    const unsigned int nTrama = 5;
    char* ptrToken;
    char* ptrSubToken;
    unsigned int tokenLen=0;

    switch(this->carac)
    {
        case 'x':
            ptrToken = strchr(&rx_buffer[read_index],'x');

            if(ptrToken)
            {			//espera a que venga una x
                ptrSubToken = strchr(ptrToken+1,'x');	//ve si hay otra x

                if(!ptrSubToken) //si no existen dos x estamos en el final del buffer
                {
                    if(strlen(ptrToken)>= nTrama)	//ver hay si n bytes
                    {
                        validarToken(ptrToken+1, 'x');	//pasa el valor sin la x
                        read_index+=nTrama; 		//solo suma si estan los valores
                        //entonces si falta vuelve a revisar
                        //porque read_index queda en el mismo valor
                    }
                }
                else 	//hay otra x
                {
                    tokenLen = ptrSubToken-ptrToken;

                    if(tokenLen==nTrama)	//la x no cuenta, tiene que ser la cantidad justa
                    {
                        read_index+=4;
                        validarToken(ptrToken+1, 'x');	//pasa el valor sin la x
                    }
                    else			//faltan datos pero ya vino otra x, error!!
                    {
                        //faltan o sobran numeros pero ya vino otra z error!!!
                        //se descarta valor
                        read_index += tokenLen;
                    }
                }
            }
            break;
        case 'y':
            //PARA Y
            ptrToken = strchr(&rx_buffer[read_index],'y');

            if(ptrToken)
            {
                //espera a que venga una y
                ptrSubToken = strchr(ptrToken+1,'y');	//ve si hay otra y

                if(!ptrSubToken) //si no existen dos y estamos en el final del buffer
                {
                    if(strlen(ptrToken)>= nTrama)	//ver hay si n bytes
                    {
                        validarToken(ptrToken+1, 'y');	//pasa el valor sin la y
                        read_index+=nTrama; 		//solo suma si estan los valores
                        //entonces si falta vuelve a revisar
                        //porque read_index queda en el mismo valor
                    }
                }
                else 	//hay otra y
                {
                    tokenLen = ptrSubToken-ptrToken;

                    if(tokenLen==nTrama)	//la y no cuenta, tiene que ser la cantidad justa
                    {
                        read_index+=4;
                        validarToken(ptrToken+1, 'y');	//pasa el valor sin la y
                    }
                    else			//faltan datos pero ya vino otra y, error!!
                    {
                        read_index += tokenLen;
                    }
                }
            }
            break;
        case 'z':
            //PARA Z
            ptrToken = strchr(&rx_buffer[read_index],'z');

            if(ptrToken)
            {
                //espera a que venga una z
                ptrSubToken = strchr(ptrToken+1,'z');	//ve si hay otra z

                if(!ptrSubToken) //si no existen dos z estamos en el final del buffer
                {
                    if(strlen(ptrToken)>= nTrama)	//ver hay si n bytes
                    {
                        validarToken(ptrToken+1, 'z');	//pasa el valor sin la z
                        read_index+=nTrama; 		//solo suma si estan los valores
                        //entonces si falta vuelve a revisar
                        //porque read_index queda en el mismo valor
                    }
                }
                else 	//hay otra z
                {
                    tokenLen = ptrSubToken-ptrToken;

                    if(tokenLen==nTrama)	//la z no cuenta, tiene que ser la cantidad justa
                    {
                        read_index+=4;
                        validarToken(ptrToken+1, 'z');	//pasa el valor sin la z
                    }
                    else			//faltan datos pero ya vino otra z, error!!
                    {
                        read_index += tokenLen;
                    }
                }
            }
            break;
    }

    if(read_index >= 1000000) read_index = 0;
}
 

double* Buffer::getValoresX(int max)
{
    //El IF acomoda la señal para cuando se desplaza del
    //centro visible por demoras en el refresco de la aplicacion
    if(values_arrayX[values_read_indexX+max] > 0)
    {
        values_read_indexX += 50;
    }
    else if(values_arrayX[values_read_indexX+100] == 0)
    {
        values_read_indexX -= 50;
    }
    if(values_read_indexX == 200000) values_read_indexX = 0;
    this->values_read_indexX++;
    return &values_arrayX[values_read_indexX];
}

double* Buffer::getValoresY(int max)
{
    //El IF acomoda la señal para cuando se desplaza del
    //centro visible por demoras en el refresco de la aplicacion
    if(values_arrayY[values_read_indexY+max] > 0)
    {
        values_read_indexY += 50;
    }
    else if(values_arrayY[values_read_indexY+100] == 0)
    {
        values_read_indexY -= 50;
    }
    if(values_read_indexY == 200000) values_read_indexY = 0;
    this->values_read_indexY++;
    return &values_arrayY[values_read_indexY];
}

double* Buffer::getValoresZ(int max)
{
    //El IF acomoda la señal para cuando se desplaza del
    //centro visible por demoras en el refresco de la aplicacion
    if(values_arrayZ[values_read_indexZ+max] > 0)
    {
        values_read_indexZ += 50;
    }
    else if(values_arrayZ[values_read_indexZ+100] == 0)
    {
        values_read_indexZ -= 50;
    }
    if(values_read_indexZ == 200000) values_read_indexZ = 0;
    this->values_read_indexZ++;
    return &values_arrayZ[values_read_indexZ];
}

double* Buffer::vectorAceleracion()
{
    return algoritmos.vectorAceleracion();
}

double* Buffer::vectorVelocidad()
{
    return algoritmos.vectorVelocidad();
}

double* Buffer::vectorPosicion()
{
    return algoritmos.vectorPosicion();
}



//FUNCIONES DE LECTURA DEL PUERTO SERIE
int Buffer::hexToInt(const char* hexStr)
{
    int hexInt;
    sscanf(hexStr, "%x", &hexInt);

    return hexInt;
}

int Buffer::writeport(int fd, char *chars) {
    int len = strlen(chars);
    printf("Enviados: %dB\r\n",len);

    chars[len] = 0x0d; // stick a <CR> after the command
    chars[len+1] = 0x00; // terminate the string properly
    int n = write(fd, chars, strlen(chars));
    if (n < 0) {
        fputs("write failed!\n", stderr);
        return 0;
    }

    return 1;
}

int Buffer::readport(int fd, char *result) {
    int iIn = read(fd, result, 254);

    if (iIn < 0) {
        if (errno == EAGAIN) {
            //no hay datos todavia
            return 0;
        } else {
            printf("SERIAL read error %d %s\n", errno, strerror(errno));
            return 0;
        }
    }

    memcpy(&rx_buffer[write_index],result, iIn);
    write_index += iIn;
    if(write_index >= 1000000) write_index = 0;

    return 1;
}

int getbaud(int fd) {
    struct termios termAttr;
    int inputSpeed = -1;
    speed_t baudRate;
    tcgetattr(fd, &termAttr);
    /* Get the input speed.                              */
    baudRate = cfgetispeed(&termAttr);
    switch (baudRate) {
            case B0:      inputSpeed = 0; break;
            case B50:     inputSpeed = 50; break;
            case B110:    inputSpeed = 110; break;
            case B134:    inputSpeed = 134; break;
            case B150:    inputSpeed = 150; break;
            case B200:    inputSpeed = 200; break;
            case B300:    inputSpeed = 300; break;
            case B600:    inputSpeed = 600; break;
            case B1200:   inputSpeed = 1200; break;
            case B1800:   inputSpeed = 1800; break;
            case B2400:   inputSpeed = 2400; break;
            case B4800:   inputSpeed = 4800; break;
            case B9600:   inputSpeed = 9600; break;
            case B19200:  inputSpeed = 19200; break;
            case B38400:  inputSpeed = 38400; break;
            case B115200: inputSpeed = 115200; break;
    }
    return inputSpeed;
}

int Buffer::initport(int fd) {
    struct termios options;
    // Get the current options for the port...
    tcgetattr(fd, &options);
    // Set the baud rates to 19200...
    cfsetispeed(&options, B115200);
    cfsetospeed(&options, B115200);
    // Enable the receiver and set local mode...
    options.c_cflag |= (CLOCAL | CREAD);

    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;

    //TODO
    //esto habria que ver porque a veces medio que se cuelga

    //options.c_cflag &= ~(ICANON | ISIG);	//agregado
    //raw input set.
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);	//original
    //	options.c_lflag &= ~(  ECHO | ECHOE | ISIG);

    // Set the new options for the port...
    tcsetattr(fd, TCSANOW, &options);
    //end hardware init


    read_index = 0;
    write_index = 0;
    return 1;
}


void Buffer::run()
{

    fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd == -1) {
            perror("open_port: Unable to open /dev/ttyS0 - ");
            return;
    } else {
            fcntl(fd, F_SETFL, 0);
    }

    initport(fd);
    //cout << "Recibiendo.." << endl;

    //fcntl(fd, F_SETFL, 0);		 //blck serial read
    fcntl(fd, F_SETFL, FNDELAY); // don't block serial read

    while(continuar){
        readport(fd,sResult);

        if((write_index-read_index)>5){
            procesar_trama();
        }
    };

    close(fd);
}




	
